function [associations, is_new] = data_association(observations, robot_pose, landmarks, landmark_ids, P_landmarks, R, threshold)
% 基于马氏距离的数据关联（改进版）
% 输入:
%   observations - 观测矩阵 [range; bearing] 每列一个观测
%   robot_pose - 机器人位姿 [x; y; theta]
%   landmarks - 已知路标位置 [x; y] 每行一个路标
%   landmark_ids - 路标ID数组
%   P_landmarks - 路标协方差，cell数组（EKF）或[]（其他）
%   R - 观测噪声协方差
%   threshold - 马氏距离阈值（默认自适应）
% 输出:
%   associations - 关联结果，每个观测对应的路标索引（-1表示新路标）
%   is_new - 布尔数组，标记是否为新路标

    % 自适应阈值策略（解决路标过多问题）
    if nargin < 7 || isempty(threshold)
        if isempty(landmarks) || size(landmarks, 1) < 5
            % 路标少时，使用中等阈值
            threshold = 9.21;  % 99%置信度
        else
            % 路标多时，放宽阈值（倾向于复用已有路标）
            threshold = 13.82;  % 99.9%置信度
        end
    end
    
    n_obs = size(observations, 2);
    associations = -ones(1, n_obs);  % 默认为新路标
    is_new = true(1, n_obs);
    
    % 如果没有已知路标，全部标记为新路标
    if isempty(landmarks) || size(landmarks, 1) == 0
        return;
    end
    
    x = robot_pose(1);
    y = robot_pose(2);
    theta = robot_pose(3);
    
    % 对每个观测进行数据关联
    for i = 1:n_obs
        z_obs = observations(:, i);  % [range; bearing]
        
        min_distance = inf;
        best_match = -1;
        best_innovation = [];
        
        % 与所有已知路标比较
        for j = 1:size(landmarks, 1)
            lm_pos = landmarks(j, :);  % [mx, my]
            
            % 预测观测
            dx = lm_pos(1) - x;
            dy = lm_pos(2) - y;
            q = dx^2 + dy^2;
            
            if q < 1e-6
                continue;  % 避免数值问题
            end
            
            range_pred = sqrt(q);
            bearing_pred = wrapToPi(atan2(dy, dx) - theta);
            z_pred = [range_pred; bearing_pred];
            
            % 计算创新
            innovation = z_obs - z_pred;
            innovation(2) = wrapToPi(innovation(2));
            
            % 观测雅可比
            H = zeros(2, 2);
            H(1, 1) = dx / range_pred;
            H(1, 2) = dy / range_pred;
            H(2, 1) = -dy / q;
            H(2, 2) = dx / q;
            
            % 创新协方差
            if ~isempty(P_landmarks) && length(P_landmarks) >= j
                % EKF-SLAM：使用路标协方差
                P_lm = P_landmarks{j};
                S = H * P_lm * H' + R;
            else
                % FastSLAM/Graph-SLAM：使用放大的观测噪声（保守估计）
                S = R * 1.5;
            end
            
            % 确保S对称正定
            S = (S + S') / 2;
            S = S + eye(2) * 1e-6;
            
            % 计算马氏距离
            try
                mahal_dist = innovation' * (S \ innovation);
            catch
                continue;  % S不可逆，跳过
            end
            
            % 额外的欧几里得距离检查（防止离群匹配）
            range_error = abs(innovation(1));
            bearing_error = abs(innovation(2));
            
            % 找到最小距离的匹配（同时满足距离和角度约束）
            if mahal_dist < threshold && range_error < 1.0 && bearing_error < 0.4
                if mahal_dist < min_distance
                    min_distance = mahal_dist;
                    best_match = j;
                    best_innovation = innovation;
                end
            end
        end
        
        % 如果找到有效匹配，额外验证
        if best_match > 0
            % 最终检查：确保不是偶然匹配
            if min_distance < threshold * 0.8 || size(landmarks, 1) > 10
                associations(i) = best_match;
                is_new(i) = false;
            end
        end
    end
end

